function p = least_square_new(dis,n)
%最小二乘（新）
%输入四个距离,n组，输出位置
p=zeros(n,3);
x0=0;y0=0;z0=1.2;
x1=x0+0.95;
y1=y0;
z1=z0;
x2=x0;
y2=y0+0.7;
z2=z0;
x3=x0+0.95;
y3=y0+0.7;
z3=z0;
for i=1:n
    d0=dis(i,1);
    d1=dis(i,2);
    d2=dis(i,3);
    d3=dis(i,4);
    A=[1,-2*x0,-2*y0;
       1,-2*x1,-2*y1;
       1,-2*x2,-2*y2;
       1,-2*x3,-2*y3];  %最后一列不能要（都是0）
    B=[d0^2-x0^2-y0^2-z0^2;
       d1^2-x1^2-y1^2-z1^2;
       d2^2-x2^2-y2^2-z2^2;
       d3^2-x3^2-y3^2-z3^2];
    X=(A'*A)^(-1)*A'*B;
    xx=X(2);
    yy=X(3);
    zz2=X(1)-xx^2-yy^2;
    zz=sqrt(abs(zz2));
    p(i,:)=[xx,yy,zz];
end
end

